#include <iostream>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <thread>
#include "serial_kalman.h"
#include <chrono>
#include <iomanip>
#include <modbus/modbus.h>

const int RETRY_DELAY_MS = 1000; // Delay between connection retries

// Convert various data types to 8-bit byte vector
template <typename T>
std::vector<uint8_t> to_byte_vector(const T &value)
{
    std::vector<uint8_t> bytes(sizeof(T));
    std::memcpy(bytes.data(), &value, sizeof(T));
    return bytes;
}

bool connect_modbus(modbus_t *&mb)
{
    while (true)
    {
        if (modbus_connect(mb) == -1)
        {
            std::cerr << "Connection failed: " << modbus_strerror(errno) << ". Retrying..." << std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(RETRY_DELAY_MS));
        }
        else
        {
            std::cout << "Connected successfully!" << std::endl;
            if (modbus_set_slave(mb, 1) == -1)
            {
                std::cerr << "Modbus set slave failed: " << modbus_strerror(errno) << std::endl;
                modbus_free(mb);
                return false;
            }
            return true;
        }
    }
}

int main(int argc, char *argv[])
{
    if (argc != 2)
    {
        std::cerr << "Usage: " << argv[0] << " <server_ip>" << std::endl;
        return 1;
    }

    std::string server_ip = argv[1];

    std::cout << " +----------------------------------+" << std::endl;
    std::cout << " |         MMW Measurement          |" << std::endl;
    std::cout << " +----------------------------------+" << std::endl;

    int mode;
    std::cout << "Select Mode (1 only measure; 2 measure and send): ";
    std::cin >> mode;

    std::vector<uint8_t> write_buffer = {0x01, 0x03, 0x00, 0x00, 0x00, 0x01, 0x84, 0x0A};
    SerialWrapper serial_wrapper("/dev/ttyUSB0", 115200, boost::asio::serial_port_base::character_size(8),
                                 boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none),
                                 boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one), 10);

    if (serial_wrapper.open_serial())
    {
        if (mode == 1)
        {
            auto empty_callback = [](uint64_t, int, double, int) {};
            std::thread send_thread(&SerialWrapper::write_data, &serial_wrapper, std::ref(write_buffer));
            std::thread receive_thread(&SerialWrapper::read_data, &serial_wrapper);
            std::thread speed_thread(&SerialWrapper::calculate_speed, &serial_wrapper, empty_callback);

            send_thread.join();
            receive_thread.join();
            speed_thread.join();
        }
        else if (mode == 2)
        {
            modbus_t *mb = modbus_new_tcp(server_ip.c_str(), MODBUS_TCP_DEFAULT_PORT);
            if (!mb || !connect_modbus(mb))
            {
                return -1;
            }

            uint16_t tab_reg[32];
            auto speed_callback = [&mb, &tab_reg](uint64_t timestamp, int current_measurement, double speed, int dir)
            {
                tab_reg[0] = timestamp >> 48;
                tab_reg[1] = timestamp >> 32;
                tab_reg[2] = timestamp >> 16;
                tab_reg[3] = timestamp;
                tab_reg[4] = current_measurement;
                std::memcpy(&tab_reg[5], &speed, sizeof(speed));
                tab_reg[9] = dir;

                if (modbus_write_registers(mb, 0, 10, tab_reg) == -1)
                {
                    std::cerr << "Failed to write to register: " << modbus_strerror(errno) << std::endl;
                    modbus_close(mb);
                    connect_modbus(mb); // Retry connection on failure
                }
                else
                {
                    std::cout << "Data sent to Modbus server" << std::endl;
                }
            };

            std::thread send_thread(&SerialWrapper::write_data, &serial_wrapper, std::ref(write_buffer));
            std::thread receive_thread(&SerialWrapper::read_data, &serial_wrapper);
            std::thread speed_thread(&SerialWrapper::calculate_speed, &serial_wrapper, speed_callback);

            send_thread.join();
            receive_thread.join();
            speed_thread.join();

            modbus_close(mb);
            modbus_free(mb);
        }
        else
        {
            std::cerr << "Invalid mode selected." << std::endl;
        }
    }

    serial_wrapper.close();
    std::cout << "\nSerial communication ended" << std::endl;

    return 0;
}
